/**
 * 用于将避障规划器输出转换为无人机期望速度信息
 */

#include "navigation_transformer.h"

using namespace std;
using namespace Eigen;

NavigationTransformer::NavigationTransformer(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private) : nh_(nh),
																											 nh_private_(nh_private)
{
	initialize();

	cmdloop_timer_ = nh_.createTimer(ros::Duration(0.1), &NavigationTransformer::CmdLoopCallback, this);
	cmd_vel_sub_ = nh_private_.subscribe("", 1, &NavigationTransformer::CmdVelCallback, this, ros::TransportHints().tcpNoDelay());
}

NavigationTransformer::~NavigationTransformer()
{
	// Destructor
}

/**
 * @brief 接收规划的速度信息（按定时器触发）
*/
void NavigationTransformer::CmdLoopCallback(const ros::TimerEvent &event)
{
	PublishVelControl();
}

/**
 * @brief 发送速度信息
*/
void NavigationTransformer::PublishVelControl()
{

}

/**
 * @brief 接收当前速度信息，需要传递给规划器运算
*/
void NavigationTransformer::CmdVelCallback(const geometry_msgs::Twist &msg)
{
	px4_vel_[0] = msg.linear.x;
	px4_vel_[1] = msg.linear.y;
}

void NavigationTransformer::initialize()
{
	px4_vel_[0] = 0;
	px4_vel_[1] = 0;
	// 读取offboard模式下飞机的期望高度
	nh_.param<float>("desire_posz_", desire_posz_, 1.0);
}


int main(int argc, char **argv)
{
	ros::init(argc, argv, "ros_nav_quadrotor");
	ros::NodeHandle nh("");
	ros::NodeHandle nh_private("~");
	NavigationTransformer NavigationTransformer(nh, nh_private);
	ros::spin();
	return 0;
}